// This is a simple test sketch which increases the targetHeading about 25degress every 1.4seconds.
// Afterwards the tracker should point into this direction.
//
// Please note that you will have to disable DEBUG in config.h otherwise the serial classes will conflict.

#include <EEPROM.h>
#include <Wire.h>
#include "compass.h"
#include "servos.h"

uint16_t targetHeading = 0;
uint16_t heading = 0;
unsigned long time;


//PID stuff
long Error[10];
long Accumulator;
long PID;
uint8_t Divider = 10;
int PWMOutput;

void setup()
{
  Wire.begin();
  initServos();
  initCompass();
  Serial.begin(57600);
  time = millis();
}

#define filterSamples 7
int currentHeadingSmoothArray [filterSamples];

void loop()
{

  if (millis() > time){
    time = millis() + 14;
    heading = digitalSmooth(getHeading(), currentHeadingSmoothArray);

    Serial.print("H:");
    Serial.print(heading);
    Serial.print(" T: ");
    Serial.print(targetHeading);

    getError();       // Get position error
    calculatePID();   // Calculate the PID output from the error
    SET_PAN_SERVO_SPEED(PWMOutput);
 }

  while (Serial.available() > 0) {
    char c = Serial.read();
    if (c == 'H' || c == 'h'){
      int speed = Serial.parseInt(); 
      targetHeading = speed;
    } 
  }
}

void getError(void)
{
  // shift error values
  for(byte i = 0; i < 10; i++){
    Error[i+1] = Error[i];
  }
  
  int16_t delta = targetHeading - heading;
  if (delta > 180){
    delta -= 360; 
  } else if (delta < -180){
    delta += 360;
  }
  // load new error into top array spot  
  Error[0] = delta;
}

void calculatePID(void)
{
  // Calculate the PID  
  PID = Error[0] * P;     // start with proportional gain
  Accumulator += Error[0];  // accumulator is sum of errors
  PID += I * Accumulator; // add integral gain and error accumulation
  PID += D * (Error[0]-Error[9]); // differential gain comes next
  PID = PID >> Divider; // scale PID down with divider

  // limit the PID to the resolution we have for the PWM variable
  if(PID >= 500)
    PID = 500;
  if(PID <= -500)
    PID = -500;

  int16_t delta = targetHeading - heading;
    if (PID > 0){
    PWMOutput = (PID + PAN_0);
    if (abs(delta) > 1)
      PWMOutput +=40;
  }else{
    PWMOutput = (PID + PAN_0);
    if (abs(delta) > 1)
      PWMOutput -=40;
  }
}

int digitalSmooth(int rawIn, int *sensSmoothArray){     // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int temp;
  uint8_t j, k, top, bottom;
  long total;
  static uint8_t i;
  static int sorted[filterSamples];
  boolean done;

  i = (i + 1) % filterSamples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn;                 // input new data into the oldest slot

  for (j=0; j<filterSamples; j++){     // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (filterSamples - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }
  bottom = max(((filterSamples * 15)  / 100), 1); 
  top = min((((filterSamples * 85) / 100) + 1  ), (filterSamples - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
  }
  return total / k;    // divide by number of samples
}
